/*
 * pid_control.c
 *
 *  Created on: Nov 20, 2024
 *      Author: Vinson
 */
#include "pid_control.h"
#include "micros.h"
#include "stdlib.h"
#include "utils.h"
#include "gpio.h"
extern gpio_t dbg_out2;


/*
 * Uq = P + I + D;
 */

 /* @brief  ：pid 计算
  * @name   ：pid_control_calculate
  * @param  ：pid。pid 对象
  * @param  : err。pid 误差
  * @return ：返回运算的结果
  * @details：给出一个误差因子，运算出pid 的结果
  */
static float pid_control_calculate(pid_control_t *pid, float err)
{
	//gpio_set_value(&dbg_out2, 0);
	float p = pid->p;
	float i = pid->i;
	float d = pid->d;

	float voltage_ramp = pid->voltage_ramp;
	float voltage_limit = pid->voltage_limit;

	/* Step1: 计算两次运行的时间差 */
	unsigned long t_now = micros();
	float Ts = (t_now - pid->ts_prev) * 1e-6f;
	if (Ts <= 0 || Ts > 0.5f)
		Ts = 1e-3f;

	/* Step2：P 环 f = ke(t) */
    float proportional = p * err;

	/* Step3：I 环 ：散点积分 Tustin
	 * S = Ts * (err + error_prev) / 2 梯形计算
	 */
	float integral = pid->integral_prev + i * Ts * 0.5f * (err + pid->error_prev);
    integral = clip(integral, -voltage_limit, voltage_limit);

	/*
	 * Step4: D 环：
	 */
	float derivative = d * (err - pid->error_prev) / Ts;

	/* Step5: 将P I D 三环计算的值加起来 */
	float output_voltage = proportional + integral + derivative;
	output_voltage = clip(output_voltage, -voltage_limit, voltage_limit);

	/* Step6：对pid 变化率进行限制 */
	if (voltage_ramp > 0) {
	float output_rate = (output_voltage - pid->voltage_prev) / Ts;
	if (output_rate > voltage_ramp)
		output_voltage = pid->voltage_prev + voltage_ramp * Ts;
	else if (output_rate < -voltage_ramp)
		output_voltage = pid->voltage_prev - voltage_ramp * Ts;
      }

	/* Step6：保存下一次运算的值 */
	pid->integral_prev = integral;
	pid->voltage_prev = output_voltage;
	pid->error_prev = err;
	pid->ts_prev = t_now;
	//gpio_set_value(&dbg_out2, 1);
	return output_voltage;
}

/* @brief  ：pid 控制器初始化
 * @name   ：pid_control_init
 * @param  ：pid
 * @return ： 无
 * @details：无
 */
static void pid_control_init(pid_control_t *pid)
{
	assert_param(pid);
	pid->error_prev = 0.0f;
	pid->voltage_prev = 0.0f;
	pid->integral_prev = 0.0f;
	pid->ts_prev = micros();
}

static void pid_control_set(pid_control_t *pid, float p, float i, float d, float voltage_ramp, float voltage_limit)
{
	assert_param(pid);
	pid->p = p;
	pid->i = i;
	pid->d = d;
	pid->voltage_ramp = voltage_ramp;
	pid->voltage_limit = voltage_limit;

}

/* @brief  ：创建一个pid 设备对象
 * @name   ：pid_control_create
 * @param  ：p, 比例系数
 * @param  ：i, 积分系数
 * @param  ：d, 微分系数
 * @param  ：voltage_ramp, 变化率限幅
 * @param  ：limit, 输出限复
 * @return ： 返回设备对象
 * @details：创建设备对象并初始化
 */
pid_control_t * pid_control_create(float p, float i, float d, float voltage_ramp, float voltage_limit)
{
	pid_control_t *pid = (pid_control_t *)malloc(sizeof(pid_control_t));
	assert_param(pid);

	pid->p = p;
	pid->i = i;
	pid->d = d;
	pid->voltage_ramp = voltage_ramp;
	pid->voltage_limit = voltage_limit;
	pid->init = pid_control_init;
	pid->pid_control = pid_control_calculate;
	pid->set = pid_control_set;

	/* pid 控制器初始化 */
	pid->init(pid);
	return pid;
}

/* @brief  ：销毁一个pid 设备对象
 * @name   ：pid_control_destory
 * @param  ：pid
 * @return ： 无
 * @details：无
 */
void pid_control_destory(pid_control_t *pid)
{
	assert_param(pid);
	free(pid);
	pid = NULL;
}

